﻿#include "UAVSimulation.h"
using namespace std;
using namespace ASL;

// 构造函数
UAVMass::UAVMass()
{
    // 初始化输入量
    this->input.fuelUsed = 0;

    // 初始化输出量
    this->output.mass = 0;
    this->output.CG.init(3);
    this->output.Inertia.init(3, 3);

    // 初始化输参数量
    this->parameter.mass = 200.0;

    this->parameter.CG.init(3);
    this->parameter.CG(AXIS_X) = 1.05;
    this->parameter.CG(AXIS_Y) = 0;
    this->parameter.CG(AXIS_Z) = 0;

    this->parameter.Inertia.init(3, 3);
    this->parameter.Inertia(AXIS_X, AXIS_X) = 8.56;
    this->parameter.Inertia(AXIS_Y, AXIS_Y) = 91.26;
    this->parameter.Inertia(AXIS_Z, AXIS_Z) = 91.26;
}

// 析构函数
UAVMass::~UAVMass()
{
}

// 初始化函数
void UAVMass::initialize(void)
{
    // 调用单步计算函数刷新输出量
    this->step();
}

// 单步运行函数
void UAVMass::step(void)
{
    // 此处默认飞行器质量参数为常值
    // 亦可补充质量参数得动态变化规律
    this->output.mass = this->parameter.mass;
    this->output.CG = this->parameter.CG;
    this->output.Inertia = this->parameter.Inertia;
}

// 构造函数
UAVAeroDynamic::UAVAeroDynamic()
{
    // 初始化输入量
    this->input.dynamicPressure = 0;
    this->input.velocity = 0;
    this->input.mach = 0;
    this->input.alpha = 0;
    this->input.beta = 0;
    this->input.omega.init(3);
    this->input.delta.init(3);
    this->input.CG.init(3);

    // 初始化输出量
    this->output.Cxyz.init(3);
    this->output.Mxyz.init(3);
    this->output.force.init(3);
    this->output.torque.init(3);

    // 初始化参数量
    this->parameter.L = 0.76;
    this->parameter.S = 0.05;

    this->parameter.CR.init(3);
    this->parameter.CR(AXIS_X) = 1.37;
    this->parameter.CR(AXIS_Y) = 0;
    this->parameter.CR(AXIS_Z) = 0;

    // 初始化临时量
    this->cV2B.init(3, 3);
    this->vCxyz.init(3);
    this->vMxyz.init(3);

    // 气动数据列表
    this->data.vMach[0] = 0.50;
    this->data.vMach[1] = 0.75;
    this->data.vMach[2] = 1.00;
    this->data.vMach[3] = 1.25;
    this->data.vMach[4] = 1.50;

    this->data.vCxMach[0] = -0.7728;
    this->data.vCxMach[1] = -0.7529;
    this->data.vCxMach[2] = -0.7328;
    this->data.vCxMach[3] = -0.7017;
    this->data.vCxMach[4] = -0.6590;

    this->data.vCyMach[0] = 0.3552;
    this->data.vCyMach[1] = 0.3335;
    this->data.vCyMach[2] = 0.3238;
    this->data.vCyMach[3] = 0.3013;
    this->data.vCyMach[4] = 0.2780;

    this->data.vCyAlpha[0] = 0.4134;
    this->data.vCyAlpha[1] = 0.3523;
    this->data.vCyAlpha[2] = 0.3094;
    this->data.vCyAlpha[3] = 0.2843;
    this->data.vCyAlpha[4] = 0.2972;

    this->data.vCzBeta[0] = -0.3237;
    this->data.vCzBeta[1] = -0.2454;
    this->data.vCzBeta[2] = -0.1996;
    this->data.vCzBeta[3] = -0.1710;
    this->data.vCzBeta[4] = -0.1618;

    this->data.vMxBeta[0] = 0.0058;
    this->data.vMxBeta[1] = 0.0011;
    this->data.vMxBeta[2] = 0.0000;
    this->data.vMxBeta[3] = 0.0000;
    this->data.vMxBeta[4] = 0.0000;

    this->data.vMxDeltaX[0] = -0.0531;
    this->data.vMxDeltaX[1] = -0.0431;
    this->data.vMxDeltaX[2] = -0.0354;
    this->data.vMxDeltaX[3] = -0.0307;
    this->data.vMxDeltaX[4] = -0.0302;

    this->data.vMyBeta[0] = -0.0519;
    this->data.vMyBeta[1] = -0.0470;
    this->data.vMyBeta[2] = -0.0442;
    this->data.vMyBeta[3] = -0.0444;
    this->data.vMyBeta[4] = -0.0421;

    this->data.vMyDeltaY[0] = -0.3237;
    this->data.vMyDeltaY[1] = -0.2454;
    this->data.vMyDeltaY[2] = -0.1996;
    this->data.vMyDeltaY[3] = -0.1710;
    this->data.vMyDeltaY[4] = -0.1618;

    this->data.vMzAlpha[0] = -0.0391;
    this->data.vMzAlpha[1] = -0.0355;
    this->data.vMzAlpha[2] = -0.0323;
    this->data.vMzAlpha[3] = -0.0312;
    this->data.vMzAlpha[4] = -0.0305;

    this->data.vMzDeltaZ[0] = -0.2689;
    this->data.vMzDeltaZ[1] = -0.2069;
    this->data.vMzDeltaZ[2] = -0.1758;
    this->data.vMzDeltaZ[3] = -0.1551;
    this->data.vMzDeltaZ[4] = -0.1413;
}

// 析构函数
UAVAeroDynamic::~UAVAeroDynamic()
{
}

// 初始化函数
void UAVAeroDynamic::initialize(void)
{
    // 初始化气动数据插值表对象
    this->vCxMach.initialize(5, this->data.vMach, 5, this->data.vCxMach);

    this->vCyMach.initialize(5, this->data.vMach, 5, this->data.vCyMach);
    this->vCyAlpha.initialize(5, this->data.vMach, 5, this->data.vCyAlpha);

    this->vCzBeta.initialize(5, this->data.vMach, 5, this->data.vCzBeta);

    this->vMxBeta.initialize(5, this->data.vMach, 5, this->data.vMxBeta);
    this->vMxDeltaX.initialize(5, this->data.vMach, 5, this->data.vMxDeltaX);

    this->vMyBeta.initialize(5, this->data.vMach, 5, this->data.vMyBeta);
    this->vMyDeltaY.initialize(5, this->data.vMach, 5, this->data.vMyDeltaY);

    this->vMzAlpha.initialize(5, this->data.vMach, 5, this->data.vMzAlpha);
    this->vMzDeltaZ.initialize(5, this->data.vMach, 5, this->data.vMzDeltaZ);

    // 初始化输出量
    this->output.Cxyz(0) = 0.0;
    this->output.Cxyz(1) = 0.0;
    this->output.Cxyz(2) = 0.0;

    this->output.Mxyz(0) = 0.0;
    this->output.Mxyz(1) = 0.0;
    this->output.Mxyz(2) = 0.0;

    this->output.force(0) = 0.0;
    this->output.force(1) = 0.0;
    this->output.force(2) = 0.0;

    this->output.torque(0) = 0.0;
    this->output.torque(1) = 0.0;
    this->output.torque(2) = 0.0;
}

// 单步运行函数
void UAVAeroDynamic::step(void)
{
    Vector r(3), cb(3), mb(3);
    double v;

    // 初始化临时量
    this->vCxyz(0) = 0.0;
    this->vCxyz(1) = 0.0;
    this->vCxyz(2) = 0.0;

    this->vMxyz(0) = 0.0;
    this->vMxyz(1) = 0.0;
    this->vMxyz(2) = 0.0;

    // 计算阻力系数
    v = this->vCxMach.interp(this->input.mach);
    this->vCxyz(AXIS_X) += v;

    // 计算升力系数
    v = this->vCyMach.interp(this->input.mach);
    this->vCxyz(AXIS_Y) += v;

    v = this->vCyAlpha.interp(this->input.mach);
    this->vCxyz(AXIS_Y) += v * this->input.alpha;

    // 计算侧向力系数
    v = this->vCzBeta.interp(this->input.mach);
    this->vCxyz(AXIS_Z) += v * this->input.beta;

    // 计算滚动力矩系数
    v = this->vMxBeta.interp(this->input.mach);
    this->vMxyz(AXIS_X) += v * this->input.beta;

    v = this->vMxDeltaX.interp(this->input.mach);
    this->vMxyz(AXIS_X) += v * this->input.delta(AXIS_X);

    // 计算航向力矩系数
    v = this->vMyBeta.interp(this->input.mach);
    this->vMxyz(AXIS_Y) += v * this->input.beta;

    v = this->vMyDeltaY.interp(this->input.mach);
    this->vMxyz(AXIS_Y) += v * this->input.delta(AXIS_Y);

    // 计算俯仰力矩系数
    v = this->vMzAlpha.interp(this->input.mach);
    this->vMxyz(AXIS_Z) += v * this->input.alpha;

    v = this->vMzDeltaZ.interp(this->input.mach);
    this->vMxyz(AXIS_Z) += v * this->input.delta(AXIS_Z);

    // 计算坐标系投影矩阵 速度系>机体系
    this->cV2B = Coordinate::cVec2Body(input.alpha, input.beta);

    // 计算实际重心与参考重心相对位置关系
    r = MathOperation::relativePosition(parameter.CR, input.CG);

    // 将速度系气动力系数投影至机体系
    cb = this->cV2B * this->vCxyz;

    // 计算气动力系数对于力矩系数得影响偏量
    mb = Vector::cross(r, cb);

    // 计算综合气动力系数
    this->output.Cxyz(AXIS_X) = this->vCxyz(AXIS_X);
    this->output.Cxyz(AXIS_Y) = this->vCxyz(AXIS_Y);
    this->output.Cxyz(AXIS_Z) = this->vCxyz(AXIS_Z);

    // 计算综合气动力矩系数
    this->output.Mxyz(AXIS_X) = this->vMxyz(AXIS_X) + mb(AXIS_X) / parameter.L;
    this->output.Mxyz(AXIS_Y) = this->vMxyz(AXIS_Y) + mb(AXIS_Y) / parameter.L;
    this->output.Mxyz(AXIS_Z) = this->vMxyz(AXIS_Z) + mb(AXIS_Z) / parameter.L;

    // 计算综合气动力 N
    this->output.force(AXIS_X) = cb(AXIS_X) * this->input.dynamicPressure * parameter.S;
    this->output.force(AXIS_Y) = cb(AXIS_Y) * this->input.dynamicPressure * parameter.S;
    this->output.force(AXIS_Z) = cb(AXIS_Z) * this->input.dynamicPressure * parameter.S;

    // 计算综合气动力矩 N*m
    this->output.torque(AXIS_X) = this->output.Mxyz(AXIS_X) * this->input.dynamicPressure * parameter.S * parameter.L;
    this->output.torque(AXIS_Y) = this->output.Mxyz(AXIS_Y) * this->input.dynamicPressure * parameter.S * parameter.L;
    this->output.torque(AXIS_Z) = this->output.Mxyz(AXIS_Z) * this->input.dynamicPressure * parameter.S * parameter.L;
}

// 构造函数
UAVPropulsor::UAVPropulsor()
{
    // 初始化输入量
    this->input.time = 0;
    this->input.CG.init(3);

    // 初始化输出量
    this->output.thrust = 0;
    this->output.fuelUsed = 0;
    this->output.force.init(3);
    this->output.torque.init(3);

    // 初始化参数量
    this->parameter.location.init(3);
    this->parameter.location(AXIS_X) = 2.17;
    this->parameter.location(AXIS_Y) = 0;
    this->parameter.location(AXIS_Z) = 0;

    // 初始化中间量
    this->vStartTime = 0;

    // 初始化动力性能数据
    this->data.vTime[0] = 0;
    this->data.vTime[1] = 5;
    this->data.vTime[2] = 10;
    this->data.vTime[3] = 20;
    this->data.vTime[4] = 50;
    this->data.vTime[5] = 70;
    this->data.vTime[6] = 80;
    this->data.vTime[7] = 85;
    this->data.vTime[8] = 90;
    this->data.vTime[9] = 100;

    this->data.vThrust[0] = 0;
    this->data.vThrust[1] = 1500;
    this->data.vThrust[2] = 4500;
    this->data.vThrust[3] = 5000;
    this->data.vThrust[4] = 5000;
    this->data.vThrust[5] = 4500;
    this->data.vThrust[6] = 3500;
    this->data.vThrust[7] = 2500;
    this->data.vThrust[8] = 1000;
    this->data.vThrust[9] = 0;
}

// 析构函数
UAVPropulsor::~UAVPropulsor()
{
}

// 初始化函数
void UAVPropulsor::initialize(void)
{
    // 初始化动力数据插值表
    this->vThrust.initialize(10, this->data.vTime, 10, this->data.vThrust);

    // 初始化输出量
    this->output.thrust = 0;
    this->output.fuelUsed = 0;

    this->output.force(0) = 0;
    this->output.force(1) = 0;
    this->output.force(2) = 0;

    this->output.torque(0) = 0;
    this->output.torque(1) = 0;
    this->output.torque(2) = 0;

    // 初始化中间量
    this->vStartTime = 0;
}

// 单步运行函数
void UAVPropulsor::step(void)
{
    double time;
    ASL::Vector r(3);

    // 计算当前工作时刻 s
    time = this->input.time - this->vStartTime;

    // 计算额定推力 N
    this->output.thrust = this->vThrust.interp(time);

    // 计算燃料消耗 kg
    this->output.fuelUsed = 0;

    // 计算综合推力 N 机体系
    this->output.force(AXIS_X) = this->output.thrust;
    this->output.force(AXIS_Y) = 0;
    this->output.force(AXIS_Z) = 0;

    // 计算发动机安装位置与飞行器重心得相对位置关系
    r = MathOperation::relativePosition(this->parameter.location, this->input.CG);

    // 计算综合推力矩 N*m 机体系
    this->output.torque = Vector::cross(r, this->output.force);
}

// 构造函数
UAVTarget::UAVTarget()
{
    // 初始化输入量
    this->input.time = 0;

    // 初始化输出量
    this->output.rLLA.init(3);
    this->output.vNav.init(3);

    // 初始化参数量
    this->parameter.rLLA.init(3);
    this->parameter.rLLA(AXIS_LON) = 122.0;
    this->parameter.rLLA(AXIS_LAT) = 39.0;
    this->parameter.rLLA(AXIS_ALT) = 100.0;

    this->parameter.vNav.init(3);
    this->parameter.vNav(AXIS_N) = 0;
    this->parameter.vNav(AXIS_U) = 0;
    this->parameter.vNav(AXIS_E) = 20.0;

    // 初始化中间量
    this->vStartTime = 0;
}

// 析构函数
UAVTarget::~UAVTarget()
{
}

// 初始化函数
void UAVTarget::initialize(void)
{
    // 初始化中间量
    this->vStartTime = 0;

    // 默认目标以初始位置为起点作匀速直线运动
    this->output.rLLA = this->parameter.rLLA;
    this->output.vNav = this->parameter.vNav;
}

// 单步运行函数
void UAVTarget::step(void)
{
    double time;
    double lamda, phi, height;
    double v, psiV, A, S;

    // 计算目标运动时间 s
    time = this->input.time - this->vStartTime;

    // 计算目标弹道偏角 -180~+180 deg
    psiV = Coordinate::psiV(this->output.vNav(AXIS_X), this->output.vNav(AXIS_Y), this->output.vNav(AXIS_Z));

    // 换算为大地方位角 0~360 deg
    if (psiV >= 0.0)
    {
        A = 360.0 - psiV;
    }
    else
    {
        A = 0.0 - psiV;
    }

    // 计算目标当前经纬度 deg
    v = sqrt(this->parameter.vNav(AXIS_N) * this->parameter.vNav(AXIS_N) + this->parameter.vNav(AXIS_E) * this->parameter.vNav(AXIS_E));
    S = v * time;
    Geodesy::vincentLB(this->parameter.rLLA(AXIS_LON), this->parameter.rLLA(AXIS_LAT), S, A, lamda, phi);

    // 计算目标当前海拔高度 m
    v = this->output.vNav(AXIS_U);
    height = this->parameter.rLLA(AXIS_ALT) + v * time;

    // 更新目标实时位置
    this->output.rLLA(AXIS_LON) = lamda;
    this->output.rLLA(AXIS_LAT) = phi;
    this->output.rLLA(AXIS_ALT) = height;

    // 更新目标实时速度
    this->output.vNav(AXIS_N) = this->parameter.vNav(AXIS_N);
    this->output.vNav(AXIS_U) = this->parameter.vNav(AXIS_U);
    this->output.vNav(AXIS_E) = this->parameter.vNav(AXIS_E);
}

// 构造函数
UAVGuidance::UAVGuidance()
{
    // 初始化输入量
    this->input.aircraftLLA.init(3);
    this->input.aircraftVelocity.init(3);
    this->input.targetLLA.init(3);
    this->input.targetVelocity.init(3);

    // 初始化输出量
    this->output.relativeDistance = 0;
    this->output.ApproachingSpeed = 0;
    this->output.LOSAngle.init(3);
    this->output.LOSRate.init(3);

    // 初始化中间量
    this->cN2L.init(3, 3);
}

// 析构函数
UAVGuidance::~UAVGuidance()
{
}

// 初始化函数
void UAVGuidance::initialize(void)
{
    this->output.relativeDistance = 0;
    this->output.ApproachingSpeed = 0;
    this->output.LOSAngle(0) = 0;
    this->output.LOSAngle(1) = 0;
    this->output.LOSAngle(2) = 0;
    this->output.LOSRate(0) = 0;
    this->output.LOSRate(1) = 0;
    this->output.LOSRate(2) = 0;
}

// 单步运行函数
void UAVGuidance::step(void)
{
    double S, A;
    double x, y, z;
    Vector v(3);

    // 计算飞行器与目标的大地距离和方位角
    Geodesy::vincentSA(this->input.aircraftLLA(AXIS_LON), this->input.aircraftLLA(AXIS_LAT), this->input.targetLLA(AXIS_LON), this->input.targetLLA(AXIS_LAT), S, A);

    // 计算目标相对于飞行器在导航系内的坐标值
    x = S * cos(A / RAD_TO_DEG) - 0;
    y = this->input.targetLLA(AXIS_ALT) - this->input.aircraftLLA(AXIS_ALT);
    z = S * sin(A / RAD_TO_DEG) - 0;

    // 计算相对距离 m
    this->output.relativeDistance = sqrt(x * x + y * y + z * z);

    // 计算空间视线角 deg
    this->output.LOSAngle(AXIS_X) = 0;
    this->output.LOSAngle(AXIS_Y) = -RAD_TO_DEG * atan2(z, x);
    this->output.LOSAngle(AXIS_Z) = RAD_TO_DEG * asin(y / this->output.relativeDistance);

    // 计算坐标系投影矩阵 导航系>视线系
    this->cN2L = Coordinate::rotationZ(this->output.LOSAngle(AXIS_Z)) * Coordinate::rotationY(this->output.LOSAngle(AXIS_Y));
    v = this->cN2L * (this->input.aircraftVelocity - this->input.targetVelocity);

    // 计算接近速度 m/s
    this->output.ApproachingSpeed = -v(AXIS_X);

    // 计算空间视线角速度 deg/s
    this->output.LOSRate(AXIS_X) = 0;
    this->output.LOSRate(AXIS_Y) = RAD_TO_DEG * v(AXIS_Z) / (this->output.relativeDistance * cos(this->output.LOSAngle(AXIS_Z) / RAD_TO_DEG));
    this->output.LOSRate(AXIS_Z) = -RAD_TO_DEG * v(AXIS_Y) / this->output.relativeDistance;
}

// 构造函数
UAVControlSystem::UAVControlSystem()
{
    // 初始化输入量
    this->input.time = 0;
    this->input.rLLA.init(3);
    this->input.vNav.init(3);
    this->input.eular.init(3);
    this->input.omega.init(3);
    this->input.LOSRate.init(3);

    // 初始化输出量
    this->output.altitudeCommand = 0;
    this->output.delta.init(3);
    this->output.eularCommand.init(3);
    this->output.accelerationCommand.init(3);

    // 初始化参数量
    this->parameter.cruiseAltitude = 1000.0;

    this->parameter.targetLocation.init(3);
    this->parameter.targetLocation(AXIS_LON) = 122.0;
    this->parameter.targetLocation(AXIS_LAT) = 39.0;
    this->parameter.targetLocation(AXIS_ALT) = 100.0;

    this->parameter.launchEular.init(3);
    this->parameter.launchEular(AXIS_X) = 0;
    this->parameter.launchEular(AXIS_Y) = 0;
    this->parameter.launchEular(AXIS_Z) = 0;

    // 初始化中间量
    this->vLaunchTime = 0;
    this->vCruiseTime = 0;
    this->vAttackTime = 0;
    this->vMode = 0;
    this->vLaunchEular.init(3);
    this->vCruiseEular.init(3);
    this->vAttackEular.init(3);
    this->vLaunchLLA.init(3);
    this->vCruiseLLA.init(3);
    this->vAttackLLA.init(3);
    this->cN2B.init(3, 3);
    this->cN2T.init(3, 3);
}

// 析构函数
UAVControlSystem::~UAVControlSystem()
{
}

// 初始化函数
void UAVControlSystem::initialize(double stepSize)
{
    // 默认初始状态为零控制量
    this->output.delta(AXIS_X) = 0;
    this->output.delta(AXIS_Y) = 0;
    this->output.delta(AXIS_Z) = 0;

    this->output.eularCommand(AXIS_X) = 0;
    this->output.eularCommand(AXIS_Y) = 0;
    this->output.eularCommand(AXIS_Z) = 0;

    this->output.accelerationCommand(AXIS_X) = 0;
    this->output.accelerationCommand(AXIS_Y) = 0;
    this->output.accelerationCommand(AXIS_Z) = 0;

    this->output.altitudeCommand = 0;

    // 默认初始时刻的飞行任务模式为发射模式
    this->vLaunchTime = 0;
    this->vCruiseTime = 0;
    this->vAttackTime = 0;
    this->vMode = UAVControlSystem::Launch;
    this->vLaunchEular = this->parameter.launchEular;
    this->cDeltaHeight.initialize(Calculus::Integrator, Calculus::Euler, stepSize, 0.0);
}

// 单步运行函数
void UAVControlSystem::step(void)
{
    // 计算飞行器与预设目标区域的大地距离和方位角
    Geodesy::vincentSA(this->input.rLLA(AXIS_LON), this->input.rLLA(AXIS_LAT), this->parameter.targetLocation(AXIS_LON), this->parameter.targetLocation(AXIS_LAT), this->distance, this->angle);

    // 换算为目标航向角 -180~+180 deg
    if (this->angle >= 0.0 && this->angle < 180.0)
    {
        this->angle = 0.0 - this->angle;
    }
    else
    {
        this->angle = 360.0 - this->angle;
    }

    if (this->input.time >= 55.0)
    {
        this->vMode = this->vMode;
    }

    // 当飞行时间小于30s时启用发射模式
    if (this->input.time < 20.0)
    {
        this->vMode = UAVControlSystem::Launch;
        this->vCruiseTime = this->input.time;
        this->vCruiseLLA = this->input.rLLA;
        this->vCruiseEular = this->input.eular;
        this->launch();
    }
    // 当飞行器与预设目标区域距离 R>8km 时启用巡航模式
    else if (this->distance >= 8000.0)
    {
        this->vMode = UAVControlSystem::Cruise;
        this->vAttackTime = this->input.time;
        this->vAttackLLA = this->input.rLLA;
        this->vAttackEular = this->input.eular;
        this->vAttackAcceleration = this->input.aBody;
        this->cruise();
    }
    // 当飞行器进入预设目标区域范围时启用攻击模式
    else
    {
        this->vMode = UAVControlSystem::Attack;
        this->attack();
    }
}

// 发射模式控制算法
void UAVControlSystem::launch(void)
{
    double v, z, t;

    // 滚动通道采用PD控制回路 让滚动角维持于零位
    v = 0.15 * (0 - this->input.eular(AXIS_X)) + 0.05 * (0 - this->input.omega(AXIS_X));
    this->output.eularCommand(AXIS_X) = 0;
    this->output.delta(AXIS_X) = Basic::limit(-v, -15, +15);

    // 航向通道采用PD控制回路 让航向角维持于初始朝向
    v = 0.0 * (this->vLaunchEular(AXIS_Y) - this->input.eular(AXIS_Y)) + 0.08 * (0 - this->input.omega(AXIS_Y));
    this->output.eularCommand(AXIS_Y) = this->vLaunchEular(AXIS_Y);
    this->output.delta(AXIS_Y) = Basic::limit(-v, -15, +15);

    // 人为设置俯仰角过渡函数作为参考基准
    t = this->input.time - this->vLaunchTime;
    z = 2.0 + (this->vLaunchEular(AXIS_Z) - 2.0) * exp(-t / 5.0);
    this->output.eularCommand(AXIS_Z) = z;

    // 俯仰通道采用PD控制回路 让俯仰角按预设变化规律过渡至平稳状态
    v = 0.35 * (z - this->input.eular(AXIS_Z)) + 0.15 * (0 - this->input.omega(AXIS_Z));
    this->output.delta(AXIS_Z) = Basic::limit(-v, -15, +15);
}

// 巡航模式控制算法
void UAVControlSystem::cruise(void)
{
    double a, b, c;
    double v, z, t;

    // 滚动通道采用PD控制回路 让滚动角维持于零位
    v = 0.15 * (0 - this->input.eular(AXIS_X)) + 0.05 * (0 - this->input.omega(AXIS_X));
    this->output.eularCommand(AXIS_X) = 0;
    this->output.delta(AXIS_X) = Basic::limit(-v, -15, +15);

    // 航向通道采用PD控制回路 让航向角指向预设目标区域方向
    t = this->input.time - this->vCruiseTime;
    z = this->angle + (this->vCruiseEular(AXIS_Y) - this->angle) * exp(-t / 10.0);
    v = 0.20 * (z - this->input.eular(AXIS_Y)) + 0.08 * (0 - this->input.omega(AXIS_Y));
    this->output.eularCommand(AXIS_Y) = z;
    this->output.delta(AXIS_Y) = Basic::limit(-v, -15, +15);

    // 俯仰通道采用PD控制回路 让飞行器维持于升重平衡的状态
    t = this->input.time - this->vCruiseTime;
    z = 2.0 + (this->vCruiseEular(AXIS_Z) - 2.0) * exp(-t / 5.0);
    this->output.eularCommand(AXIS_Z) = z;
    a = 0.35 * (z - this->input.eular(AXIS_Z)) + 0.15 * (0 - this->input.omega(AXIS_Z));
    // a = Basic::limit(-a, -10, +10);

    // 高度通道采用PID控制回路 让飞行器稳定飞行于预设高度
    t = this->input.time - this->vCruiseTime;
    z = this->parameter.cruiseAltitude + (this->vCruiseLLA(AXIS_ALT) - this->parameter.cruiseAltitude) * exp(-t / 10.0);
    this->output.altitudeCommand = z;
    v = z - this->input.rLLA(AXIS_ALT);
    b = 0.1 * v + 0.002 * this->cDeltaHeight.calculate(v) + 0.08 * (0.0 - this->input.vNav(AXIS_U));
    // b = Basic::limit(-b, -10, +10);

    // 综合俯仰角与高度控制指令
    this->output.delta(AXIS_Z) = Basic::limit(-a - b, -15, +15);
}

// 攻击模式控制算法
void UAVControlSystem::attack(void)
{
    double v, a, b, c, t, z;
    double theta, psiV;
    Vector AT(3), AB(3);

    // 滚动通道采用PD控制回路 让滚动角维持于零位
    v = 0.10 * (0 - this->input.eular(AXIS_X)) + 0.05 * (0 - this->input.omega(AXIS_X));
    this->output.eularCommand(AXIS_X) = 0;
    this->output.accelerationCommand(AXIS_X) = 0;
    this->output.delta(AXIS_X) = Basic::limit(-v, -15, +15);

    // 计算弹道倾角与弹道偏角 deg
    theta = Coordinate::theta(this->input.vNav(AXIS_X), this->input.vNav(AXIS_Y), this->input.vNav(AXIS_Z));
    psiV = Coordinate::psiV(this->input.vNav(AXIS_X), this->input.vNav(AXIS_Y), this->input.vNav(AXIS_Z));

    // 计算坐标系投影矩阵
    this->cN2T = Coordinate::cNav2Traj(theta, psiV);
    this->cN2B = Coordinate::cNav2Body(this->input.eular(AXIS_X), this->input.eular(AXIS_Y), this->input.eular(AXIS_Z));

    // 由比例导引法计算弹道系加速度指令 m/s^2
    v = Vector::norm(this->input.vNav);
    AT(AXIS_X) = 0;
    AT(AXIS_Y) = 3.0 * v * this->input.LOSRate(AXIS_Z) / RAD_TO_DEG;
    AT(AXIS_Z) = -3.0 * v * cos(theta / RAD_TO_DEG) * this->input.LOSRate(AXIS_Y) / RAD_TO_DEG;

    // 将弹道系加速度指令投影至机体系 m/s^2
    AB = this->cN2B * Matrix::transpose(this->cN2T) * AT;

    // 航向通道采用PD控制回路 沿理侧向想追踪轨迹运动
    t = this->input.time - this->vAttackTime;
    z = AB(AXIS_Z) + (this->vAttackAcceleration(AXIS_Z) - AB(AXIS_Z)) * exp(-t / 1.0);
    this->output.eularCommand(AXIS_Y) = 0;
    this->output.accelerationCommand(AXIS_Z) = z;

    a = 0.08 * (z - this->input.aBody(AXIS_Z));
    b = 0.20 * (0 - this->input.omega(AXIS_Y));
    v = a - b;
    this->output.delta(AXIS_Y) = Basic::limit(v, -15, +15);

    // 俯仰通道采用PD控制回路 沿纵向理想追踪轨迹运动
    t = this->input.time - this->vAttackTime;
    z = AB(AXIS_Y) + (this->vAttackAcceleration(AXIS_Y) - AB(AXIS_Y)) * exp(-t / 5.0);
    this->output.eularCommand(AXIS_Z) = 0;
    this->output.accelerationCommand(AXIS_Y) = z;
    this->output.altitudeCommand = 0;

    a = 0.01 * (z - this->input.aBody(AXIS_Y));
    b = 0.10 * (0 - this->input.omega(AXIS_Z));
    v = -a - b;
    this->output.delta(AXIS_Z) = Basic::limit(v, -15, +15);
}

// 构造函数
UAVActuator::UAVActuator()
{
    this->input.u = 0;
    this->output.y = 0;

    this->parameter.k = 0;
    this->parameter.rangeA[0] = 0;
    this->parameter.rangeA[1] = 0;
    this->parameter.rangeV[0] = 0;
    this->parameter.rangeV[1] = 0;
}

// 析构函数
UAVActuator::~UAVActuator()
{
}

// 初始化函数
bool UAVActuator::initialize(double stepSize)
{
    this->vG.initialize(Calculus::Integrator, Calculus::Euler, stepSize, 0.0);
    this->output.y = 0.0;

    return true;
}

// 单步运行函数
bool UAVActuator::step(void)
{
    double u;

    u = Basic::limit(this->input.u, parameter.rangeA[0], parameter.rangeA[1]);
    u = parameter.k * (u - this->output.y);
    u = Basic::limit(u, parameter.rangeV[0], parameter.rangeV[1]);
    this->output.y = this->vG.calculate(u);

    return true;
}

// 构造函数
UAVSimulation::UAVSimulation()
{
    startTime = 0;
    stopTime = 0;
    stepSize = 0;
    currentTime = 0;
    currentCount = 0;
}

// 析构函数
UAVSimulation::~UAVSimulation()
{
}

// 准备函数
void UAVSimulation::setup(void)
{
    cout << "正在准备仿真试验 setup" << endl;

    // 仿真求解配置
    this->startTime = 0.0;
    this->stopTime = 100.0;
    this->stepSize = 0.005;
    this->currentTime = 0.0;
    this->currentCount = 0;

    // 六自由度运动学模型参数
    this->cDOFModel.parameter.rLLA(0) = 122.0;
    this->cDOFModel.parameter.rLLA(1) = 39.0;
    this->cDOFModel.parameter.rLLA(2) = 1500.0;

    this->cDOFModel.parameter.vBody(0) = 200.0;
    this->cDOFModel.parameter.vBody(1) = 0;
    this->cDOFModel.parameter.vBody(2) = 0;

    this->cDOFModel.parameter.eular(0) = 0;
    this->cDOFModel.parameter.eular(1) = 0;
    this->cDOFModel.parameter.eular(2) = 0;

    this->cDOFModel.parameter.omega(0) = 0;
    this->cDOFModel.parameter.omega(1) = 0;
    this->cDOFModel.parameter.omega(2) = 0;

    // 副翼执行机构参数
    this->cActutorModelX.parameter.k = 20.0;
    this->cActutorModelX.parameter.rangeA[0] = -10.0;
    this->cActutorModelX.parameter.rangeA[1] = +10.0;
    this->cActutorModelX.parameter.rangeV[0] = -80.0;
    this->cActutorModelX.parameter.rangeV[1] = +80.0;

    // 方向舵执行机构参数
    this->cActutorModelY.parameter.k = 12.5;
    this->cActutorModelY.parameter.rangeA[0] = -15.0;
    this->cActutorModelY.parameter.rangeA[1] = +15.0;
    this->cActutorModelY.parameter.rangeV[0] = -60.0;
    this->cActutorModelY.parameter.rangeV[1] = +60.0;

    // 升降舵执行机构参数
    this->cActutorModelZ.parameter.k = 12.5;
    this->cActutorModelZ.parameter.rangeA[0] = -15.0;
    this->cActutorModelZ.parameter.rangeA[1] = +15.0;
    this->cActutorModelZ.parameter.rangeV[0] = -60.0;
    this->cActutorModelZ.parameter.rangeV[1] = +60.0;

    // 目标运动参数
    this->cTargetModel.parameter.rLLA(0) = 122.02;
    this->cTargetModel.parameter.rLLA(1) = 39.35;
    this->cTargetModel.parameter.rLLA(2) = 0.0;

    this->cTargetModel.parameter.vNav(0) = 0.0;
    this->cTargetModel.parameter.vNav(1) = 0.0;
    this->cTargetModel.parameter.vNav(2) = 10.0;

    // 飞行控制参数
    this->cControlModel.parameter.cruiseAltitude = 1000.0;
    this->cControlModel.parameter.launchEular = cDOFModel.parameter.eular;
    this->cControlModel.parameter.targetLocation = cTargetModel.parameter.rLLA;

    // 创建本地数据文件
    this->cStream.open("Example11.txt");
    this->cStream.setf(ios::showpos);
    this->cStream.setf(ios::fixed);
    this->cStream.precision(8);

    // 仿真数据标题文本
    this->cStream << "Time(s)\t";

    this->cStream << "Longitude(deg)\tLatitude(deg)\tAltitude(m)\t";
    this->cStream << "V-North(m/s)\tV-Up(m/s)\tV-East(m/s)\t";
    this->cStream << "Gamma(deg)\tPsi(deg)\tZeta(deg)\t";
    this->cStream << "Omega-X(deg/s)\tOmega-Y(deg/s)\tOmega-Z(deg/s)\t";

    this->cStream << "Mach(-)\tAlpha(deg)\tBeta(deg)\t";
    this->cStream << "Velocity(m/s)\tTheta(deg)\tPsiV(deg)\t";

    this->cStream << "C-Delta-X(deg)\tC-Delta-Y(deg)\tC-Delta-Z(deg)\t";
    this->cStream << "C-Gamma(deg)\tC-Psi(deg)\tC-Zeta(deg)\t";
    this->cStream << "C-Accel-X(m/s^2)\tC-Accel-Y(m/s^2)\tC-Accel-Z(m/s^2)\t";
    this->cStream << "C-Location-X(m)\tC-Location-Y(m)\tC-Location-Z(m)\t";

    this->cStream << "Time(s)\tDistance(m)\tSpeed(m/s)\t";
    this->cStream << "LOS-Angel-X(deg)\tLOS-Angel-Y(deg)\tLOS-Angel-Z(deg)\t";
    this->cStream << "LOS-Rate-X(deg/s)\tLOS-Rate-Y(deg/s)\tLOS-Rate-Z(deg/s)\t";

    this->cStream << "Aircraft-X(m)\tAircraft-Y(m)\tAircraft-Z(m)\t";
    this->cStream << "Target-X(m)\tTarget-Y(m)\tTarget-Z(m)\t";

    this->cStream << "Propulsor-X(N)\tPropulsor-Y(N)\tPropulsor-Z(N)\t";
    this->cStream << "Propulsor-X(N*m)\tPropulsor-Y(N*m)\tPropulsor-Z(N*m)\t";
    this->cStream << "AeroDynamic-X(N)\tAeroDynamic-Y(N)\tAeroDynamic-Z(N)\t";
    this->cStream << "AeroDynamic-X(N*m)\tAeroDynamic-Y(N*m)\tAeroDynamic-Z(N*m)\t";

    this->cStream << endl;
}

// 初始化函数
void UAVSimulation::initialize(void)
{
    cout << "正在初始化仿真条件 initialize" << endl;

    // 系列模型初始化
    this->cDOFModel.initialize(stepSize);

    this->cAtmosModel.input.altitude = cDOFModel.parameter.rLLA(2);
    this->cAtmosModel.initialize();

    this->cMassModel.input.fuelUsed = 0.0;
    this->cMassModel.initialize();

    this->cAeroModel.initialize();
    this->cControlModel.initialize(stepSize);

    this->cThrustModel.initialize();

    this->cActutorModelX.initialize(stepSize);
    this->cActutorModelY.initialize(stepSize);
    this->cActutorModelZ.initialize(stepSize);

    this->cTargetModel.initialize();
    this->cGuidanceModel.initialize();
}

// 仿真运行函数
void UAVSimulation::step(void)
{
    double v = 0.0, mach = 0.0, Q;
    double alpha = 0.0, beta = 0.0, theta = 0.0, psi = 0.0;
    Vector RA(3), RT(3), force(3), torque(3), g(3);
    double L, B, S, A;

    cout << "正在执行仿真解算 step" << endl;

    L = cDOFModel.parameter.rLLA(AXIS_LON);
    B = cDOFModel.parameter.rLLA(AXIS_LAT);

    // 仿真运行循环
    while (currentTime < stopTime)
    {
        // 更新仿真时间
        this->currentTime = currentCount * stepSize;

        // 计算飞行器与目标关于起点位置的方位坐标
        Geodesy::vincentSA(L, B, cDOFModel.output.rLLA(AXIS_LON), cDOFModel.output.rLLA(AXIS_LAT), S, A);

        RA(AXIS_X) = S * cos(A / RAD_TO_DEG);
        RA(AXIS_Y) = cDOFModel.output.rLLA(AXIS_ALT);
        RA(AXIS_Z) = S * sin(A / RAD_TO_DEG);

        Geodesy::vincentSA(L, B, cTargetModel.output.rLLA(AXIS_LON), cTargetModel.output.rLLA(AXIS_LAT), S, A);

        RT(AXIS_X) = S * cos(A / RAD_TO_DEG);
        RT(AXIS_Y) = cTargetModel.output.rLLA(AXIS_ALT);
        RT(AXIS_Z) = S * sin(A / RAD_TO_DEG);

        // 记录仿真数据
        this->cStream << currentTime << "\t";
        this->cStream << cDOFModel.output.rLLA << cDOFModel.output.vNav;
        this->cStream << cDOFModel.output.eular << cDOFModel.output.omegaR;
        this->cStream << mach << "\t" << alpha << "\t" << beta << "\t";
        this->cStream << v << "\t" << theta << "\t" << psi << "\t";

        this->cStream << cControlModel.output.delta << cControlModel.output.eularCommand;
        this->cStream << cControlModel.output.accelerationCommand;
        this->cStream << 0.0 << "\t" << cControlModel.output.altitudeCommand << "\t" << 0.0 << "\t";

        this->cStream << currentTime << "\t" << cGuidanceModel.output.relativeDistance << "\t" << cGuidanceModel.output.ApproachingSpeed << "\t";
        this->cStream << cGuidanceModel.output.LOSAngle << cGuidanceModel.output.LOSRate;
        this->cStream << RA << RT;
        this->cStream << cThrustModel.output.force << cThrustModel.output.torque;
        this->cStream << cAeroModel.output.force << cAeroModel.output.torque;

        this->cStream << endl;

        // 仿真终止条件I 海拔高度小于零
        if (cDOFModel.output.rLLA(AXIS_ALT) <= 0.0)
        {
            break;
        }
        // 仿真终止条件II 相对距离小于预定值
        else if (currentTime >= 10.0 && cGuidanceModel.output.relativeDistance <= 1.0)
        {
            break;
        }

        // 飞行运动学模型
        force = cAeroModel.output.force + cThrustModel.output.force;
        torque = cAeroModel.output.torque + cThrustModel.output.torque;
        g = Geodesy::gravity(cDOFModel.output.rLLA(AXIS_LON), cDOFModel.output.rLLA(AXIS_LAT), cDOFModel.output.rLLA(AXIS_ALT));
        g = cDOFModel.output.cN2B * g * cMassModel.output.mass;

        this->cDOFModel.input.force = force + g;
        this->cDOFModel.input.torque = torque;
        this->cDOFModel.input.mass = cMassModel.output.mass;
        this->cDOFModel.input.inertia = cMassModel.output.Inertia;
        this->cDOFModel.step();

        // 大气环境模型
        this->cAtmosModel.input.altitude = cDOFModel.output.rLLA(2);
        this->cAtmosModel.step();

        // 飞行参数解算
        theta = Coordinate::theta(cDOFModel.output.vNav(AXIS_X), cDOFModel.output.vNav(AXIS_Y), cDOFModel.output.vNav(AXIS_Z));
        psi = Coordinate::psiV(cDOFModel.output.vNav(AXIS_X), cDOFModel.output.vNav(AXIS_Y), cDOFModel.output.vNav(AXIS_Z));
        alpha = Coordinate::alpha(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        beta = Coordinate::beta(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        v = Vector::norm(cDOFModel.output.vBody);
        mach = v / cAtmosModel.output.speed;
        Q = 0.5 * cAtmosModel.output.density * v * v;

        // 质量特性模型
        this->cMassModel.input.fuelUsed = 0;
        this->cMassModel.step();

        // 气动特性模型
        this->cAeroModel.input.dynamicPressure = Q;
        this->cAeroModel.input.velocity = v;
        this->cAeroModel.input.mach = mach;
        this->cAeroModel.input.alpha = alpha;
        this->cAeroModel.input.beta = beta;
        this->cAeroModel.input.delta(AXIS_X) = cActutorModelX.output.y;
        this->cAeroModel.input.delta(AXIS_Y) = cActutorModelY.output.y;
        this->cAeroModel.input.delta(AXIS_Z) = cActutorModelZ.output.y;
        this->cAeroModel.input.omega = cDOFModel.output.omegaR;
        this->cAeroModel.input.CG = cMassModel.output.CG;
        this->cAeroModel.step();

        // 动力系统模型
        this->cThrustModel.input.time = currentTime;
        this->cThrustModel.input.CG = cMassModel.output.CG;
        this->cThrustModel.step();

        // 控制系统模型
        this->cControlModel.input.time = currentTime;
        this->cControlModel.input.rLLA = cDOFModel.output.rLLA;
        this->cControlModel.input.vNav = cDOFModel.output.vNav;
        this->cControlModel.input.aBody = cDOFModel.input.force * (1.0 / cMassModel.output.mass);
        this->cControlModel.input.eular = cDOFModel.output.eular;
        this->cControlModel.input.omega = cDOFModel.output.omegaR;
        this->cControlModel.input.LOSRate = cGuidanceModel.output.LOSRate;
        this->cControlModel.step();

        // 执行机构模型
        this->cActutorModelX.input.u = cControlModel.output.delta(0);
        this->cActutorModelX.step();
        this->cActutorModelY.input.u = cControlModel.output.delta(1);
        this->cActutorModelY.step();
        this->cActutorModelZ.input.u = cControlModel.output.delta(2);
        this->cActutorModelZ.step();

        // 目标模型
        this->cTargetModel.input.time = currentTime;
        this->cTargetModel.step();

        // 相对运动关系模型
        this->cGuidanceModel.input.aircraftLLA = cDOFModel.output.rLLA;
        this->cGuidanceModel.input.aircraftVelocity = cDOFModel.output.vNav;
        this->cGuidanceModel.input.targetLLA = cTargetModel.output.rLLA;
        this->cGuidanceModel.input.targetVelocity = cTargetModel.output.vNav;
        this->cGuidanceModel.step();

        // 更新周期计数器
        this->currentCount++;
    }

    this->cStream.close();
    cout << "已完成仿真试验流程 terminate" << endl;
}